# 人机协同控制

import pygame
import binascii
import os
import time
import socket
import sys
import threading
import struct
import asyncio
import cv2
import numpy as np
from car_controller import CarController

pygame_ready = False
def getKeyState():
	pygame.init()
	screen = pygame.display.set_mode((640, 480))
	pygame.display.set_caption('Car controlling')
	global cur_key, img, pygame_ready
	pygame_ready = True
	while not cur_key[pygame.K_ESCAPE]:
		cur_key = pygame.key.get_pressed()
		pygame.event.pump()
		# print('getKeyState()::', img is None)
		if img is not None:
			# print('getKeyState()::img blit here')
			pygame.surfarray.blit_array(screen, img)
			pygame.display.update()
	pygame.quit()
	return None

img = None
def getImage():
	global img
	while not pygame_ready: pass
	while not cur_key[pygame.K_ESCAPE]:
		img = car_controller.getImage()
		# print('getImage()::', img is None)
	return None

mannual = False
def sendCommand():
	global mannual
	while not pygame_ready: pass
	while not cur_key[pygame.K_ESCAPE]:
		motor_u_d, motor_l_r = 0, 0
		if cur_key[pygame.K_UP]:
			motor_u_d += 1
			mannual = True
		if cur_key[pygame.K_DOWN]:
			motor_u_d -= 1
			mannual = True
		if cur_key[pygame.K_LEFT]:
			motor_l_r += 1
			mannual = True
		if cur_key[pygame.K_RIGHT]:
			motor_l_r -= 1
			mannual = True
		
		if not mannual: continue
		if cur_key[pygame.K_RETURN]:
			mannual = False
		
		if car_controller.getMessageOnce()[1] != 90:
			car_controller.servoReSet()
		if motor_u_d > 0 and motor_l_r > 0:
			car_controller.motorLeftForward()
		if motor_u_d > 0 and motor_l_r == 0:
			car_controller.motorForward()
		if motor_u_d > 0 and motor_l_r < 0:
			car_controller.motorRightForward()
		if motor_u_d == 0 and motor_l_r > 0:
			car_controller.motorLeft()
		if motor_u_d == 0 and motor_l_r == 0:
			car_controller.motorStop()
		if motor_u_d == 0 and motor_l_r < 0:
			car_controller.motorRight()
		if motor_u_d < 0 and motor_l_r > 0:
			car_controller.motorLeftBackward()
		if motor_u_d < 0 and motor_l_r == 0:
			car_controller.motorBackward()
		if motor_u_d < 0 and motor_l_r < 0:
			car_controller.motorRightBackward()
	return None

def auto():
	global mannual
	while not pygame_ready or car_controller.horizontal_servo is None: pass
	while True:
		if mannual: continue
		
		distance, angle = car_controller.getMessageOnce()
		if distance >= 30:
			car_controller.motorForward()
		else:
			car_controller.motorStop()
			time.sleep(.05)
			
			if mannual: continue
			car_controller.servoTurnLeft(30)
			l_dis = car_controller.getMessageOnce()[0]
			time.sleep(1)
			if mannual: continue
			car_controller.servoTurnLeft(30)
			l_dis = max(l_dis, car_controller.getMessageOnce()[0])
			time.sleep(1)
			
			if mannual: continue
			car_controller.servoTurnRight(90)
			r_dis = car_controller.getMessageOnce()[0]
			time.sleep(1)
			if mannual: continue
			car_controller.servoTurnRight(30)
			r_dis = max(r_dis, car_controller.getMessageOnce()[0])
			time.sleep(1)
			
			if mannual: continue
			car_controller.servoTurnLeft(60)
			
			if max(l_dis, r_dis) < 30:
				mannual = True
			else:
				print('l, r:', l_dis, r_dis)
				if mannual: continue
				if l_dis > r_dis:
					car_controller.motorLeft()
				else:
					car_controller.motorRight()
				st_time = time.time()
				while car_controller.getMessageOnce()[0] < max(l_dis, r_dis) - 1:
					if mannual: break
					if time.time() - st_time > 0.5: break
				if mannual: continue
				car_controller.motorStop()
	return None

pygame.init()
cur_key = pygame.key.get_pressed()
pygame.quit()

car_controller = CarController(IP=input('ip?'), image_on=False)

print('here at main before threading')
threading.Thread(target=getKeyState, name='getKeyState').start()
threading.Thread(target=sendCommand, name='sendCommand').start()
threading.Thread(target=car_controller.receiveMessage, name='receiveMessage').start()
threading.Thread(target=auto, name='auto').start()
if car_controller.image_on: threading.Thread(target=getImage, name='getImage').start()
print(len(threading.enumerate()))

while len(threading.enumerate()) > 1:
	print(car_controller.getMessageOnce())
	time.sleep(.5)
	pass